#include<arduino.h>
#include <Servo.h>

Servo myservo; 

char serialData;
int pos;
#define gpio0 3  //舵机的信号引脚
#define gpio1 4  //小车的四个引脚
#define gpio2 2  
#define gpio3 6
#define gpio4 7  
#define en1 5     //L298N的控速引脚
#define en2 11     

#define v1 250  //前进后退速度
#define v2 250   //左转右转速度

void qianjin(){
      Serial.print("Got command: ");  
      Serial.println(serialData); 
      Serial.println("前进");

      analogWrite(en1,v1);
      analogWrite(en2,v1);
      digitalWrite(gpio1, HIGH);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, HIGH);  
      digitalWrite(gpio4, LOW);
      
}
void houtui(){
      Serial.print("Got command: ");  
      Serial.println(serialData); 
      Serial.println("后退");

      analogWrite(en1,v1); 
      analogWrite(en2,v1);
      
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, HIGH);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, HIGH);
       
}
void zuozhuan(){
      Serial.print("Got command: ");  
      Serial.println(serialData); 
      Serial.println("左转");

      analogWrite(en1,v2); 
      analogWrite(en2,v2);
      digitalWrite(gpio1, HIGH);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, HIGH);
        
}
void youzhuan(){
      Serial.print("Got command: ");  
      Serial.println(serialData); 
      Serial.println("右转");

      analogWrite(en1,v2); 
      analogWrite(en2,v2);
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, HIGH);
      digitalWrite(gpio3, HIGH);  
      digitalWrite(gpio4, LOW);
       
}
void tingzhi(){
      analogWrite(en1,0); 
      analogWrite(en2,0);
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, LOW);
      
}
void chushihua(){
      pinMode(gpio1, OUTPUT); 
      pinMode(gpio2, OUTPUT);
      pinMode(gpio3, OUTPUT);
      pinMode(gpio4, OUTPUT); 
      
      
      digitalWrite(gpio1, LOW);
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);
      digitalWrite(gpio4, LOW);
}
void bihe(){
      for (pos = 22; pos <= 90; pos ++) { // 0°到180°
        myservo.write(pos);              // 舵机角度写入
        delay(5);                       // 等待转动到指定角度
      }
}
void dakai(){
      for (pos = 90; pos >= 22; pos --) { // 从180°到0°
        myservo.write(pos);              // 舵机角度写入
        delay(5);                       // 等待转动到指定角度
      }
}
void setup() {
      Serial.begin(9600);
      myservo.attach(gpio0);  
      myservo.write(90);
}
 
void loop(){
    serialData =  Serial.read(); 
      
    switch(serialData){
      case '1': qianjin(); break;
      case '0': houtui(); break;
      case '2': zuozhuan(); break;
      case '3': youzhuan(); break;
      case '4': tingzhi(); break;
      case '5': bihe(); break;
      case '6': dakai(); break;
    }
}
